#include "cooperate_locate.h"


void compare_wc(Vector3d& v1, Vector3d& v2){
    // 以v1的经纬度为基准建立ENU坐标系
    ENUConverter converter(v1.x, v1.y);
    
    // 转换v1和v2到ENU坐标
    double x1, y1, x2, y2;
    converter.Forward(v1.x, v1.y, x1, y1);  // v1在自身坐标系中的坐标应为(0,0)
    converter.Forward(v2.x, v2.y, x2, y2);
    
    // 计算相对误差（米）
    double error_x = x2 - x1;  // 在ENU坐标系中，x2应为0
    double error_y = y2 - y1;  // 在ENU坐标系中，y2应为0
    
    std::cout << "经度误差: " << error_x << " 米" << std::endl;
    std::cout << "纬度误差: " << error_y << " 米" << std::endl;
    std::cout << "总误差: " << sqrt(error_x*error_x + error_y*error_y) << " 米" << std::endl;
    std::cout << std::endl << std::endl;
}
void test00_degreesToRadians()
{
    double x1, y1, x2, y2;
    x1 = degreesToRadians(0);
    y1 = degreesToRadians(0);
    x2 = degreesToRadians(0);
    y2 = degreesToRadians(45);

    VehicleWgs84 car1(120.1223342, 30.3422123, x1, y1);
    VehicleWgs84 car2(120.1224382, 30.3422123, x2, y2);
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}

void test01_get_obj_enu_from_car()
{
    VehicleWgs84 car1(120.1223342, 30.3422123, M_PI / 2, 0);        // (0,0)
    VehicleWgs84 car2(120.1224382, 30.3422123, M_PI / 2, M_PI / 4); // (10,0) 目标偏离角-45度
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);
}

void test02_get_obj_enu_from_drone()
{
    Vector3d drone_wgs84(0, 0, 10.28);
    Vector3d camera_pos(0.1, 0, 0);
    Vector3d pixel(321.56, 246.28, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(90, 0, 0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -45, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);
}

void test03_obj_from_enu2wgs84_car()
{
    VehicleWgs84 car1(120.1223342, 30.3422123, M_PI / 2, 0);         // (0,0)
    VehicleWgs84 car2(120.1224382, 30.3422123, M_PI / 2, -M_PI / 4); // (10,0) 目标偏离角-45度
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}

void test04_obj_from_enu2wgs84_drone()
{
    Vector3d drone_wgs84(118.8563362,32.0266519, 71.28);
    Vector3d camera_pos(0.1, 0, 0);
    Vector3d pixel(130, 372.5, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(0.7, 0, 0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);

    // 测试转经纬度
    VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);
}

void test05_locate_fusion()
{
    Vector3d obj1(120.1223342, 30.3422123, 0);
    Vector3d obj2(120.1223350, 30.3422128, 0);
    Vector3d res = locate_fusion(obj1, obj2);
    std::cout << std::fixed << std::setprecision(7);
    std::cout << res.x << ", " << res.y << std::endl;
}

void test11_car()
{
    double x1, y1, x2, y2;
    x1 = degreesToRadians(97.96); // 航向角
    y1 = degreesToRadians(18.68); // 目标偏离角
    x2 = degreesToRadians(5.36);
    y2 = degreesToRadians(4.30);

    VehicleWgs84 car1(118.8579154, 32.0266936, x1, y1);
    VehicleWgs84 car2(118.8578912, 32.0266808, x2, y2);
    Vector3d obj_enu;
    bool flag = true;
    obj_enu = get_obj_enu_from_car(car1, car2, flag);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}

void test12_car()
{
    double x1, y1, x2, y2;
    x1 = degreesToRadians(34.25); // 航向角
    y1 = degreesToRadians(-5.29); // 目标偏离角
    x2 = degreesToRadians(5.36);
    y2 = degreesToRadians(4.30);

    VehicleWgs84 car1(118.8579211, 32.0266673, x1, y1);
    VehicleWgs84 car2(118.8578912, 32.0266808, x2, y2);
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}

void test13_car()
{
    double x1, y1, x2, y2;
    x1 = degreesToRadians(33.06); // 航向角
    y1 = degreesToRadians(20.46); // 目标偏离角
    x2 = degreesToRadians(5.36);
    y2 = degreesToRadians(4.30);

    VehicleWgs84 car1(118.8579246, 32.0267120, x1, y1);
    VehicleWgs84 car2(118.8578912, 32.0266808, x2, y2);
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}

void test14_car()
{
    double x1, y1, x2, y2;
    x1 = degreesToRadians(29.16); // 航向角
    y1 = degreesToRadians(8.40); // 目标偏离角
    x2 = degreesToRadians(5.36);
    y2 = degreesToRadians(4.30);

    VehicleWgs84 car1(118.8579383, 32.0266940, x1, y1);
    VehicleWgs84 car2(118.8578912, 32.0266808, x2, y2);
    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_car(car1, car2);

    // 测试转经纬度
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
}


/* void test_car(){
    void test12_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(242.71); // 航向角
        y1 = degreesToRadians(-22.09); // 目标偏离角
        x2 = degreesToRadians(296.94);
        y2 = degreesToRadians(21.60);

        VehicleWgs84 car1(118.8579291, 32.0267016, x1, y1);
        VehicleWgs84 car2(118.8579014, 32.0266763, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test21_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(242.71); // 航向角
        y1 = degreesToRadians(-22.09); // 目标偏离角
        x2 = degreesToRadians(296.94);
        y2 = degreesToRadians(21.60);

        VehicleWgs84 car1(118.8579291, 32.0267016, x1, y1);
        VehicleWgs84 car2(118.8579014, 32.0266763, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car2, car1);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car2, obj_enu);
    }

    void test13_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(242.71); // 航向角
        y1 = degreesToRadians(-22.09); // 目标偏离角
        x2 = degreesToRadians(285.00);
        y2 = degreesToRadians(18.37);

        VehicleWgs84 car1(118.8579291, 32.0267016, x1, y1);
        VehicleWgs84 car2(118.8579557, 32.0266843, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test14_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(242.71); // 航向角
        y1 = degreesToRadians(-22.09); // 目标偏离角
        x2 = degreesToRadians(16.97);
        y2 = degreesToRadians(16.62);

        VehicleWgs84 car1(118.8579291, 32.0267016, x1, y1);
        VehicleWgs84 car2(118.8578361, 32.0266516, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test15_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(242.71); // 航向角
        y1 = degreesToRadians(-22.09); // 目标偏离角
        x2 = degreesToRadians(2.90);
        y2 = degreesToRadians(45.28);

        VehicleWgs84 car1(118.8579291, 32.0267016, x1, y1);
        VehicleWgs84 car2(118.8578657, 32.0266578, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test23_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(296.94); // 航向角
        y1 = degreesToRadians(21.60);  // 目标偏离角
        x2 = degreesToRadians(285.00);
        y2 = degreesToRadians(18.37);

        VehicleWgs84 car1(118.8579041, 32.0266763, x1, y1);
        VehicleWgs84 car2(118.8579557, 32.0266843, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test24_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(296.94); // 航向角
        y1 = degreesToRadians(21.60);  // 目标偏离角
        x2 = degreesToRadians(16.97);
        y2 = degreesToRadians(16.62);

        VehicleWgs84 car1(118.8579041, 32.0266763, x1, y1);
        VehicleWgs84 car2(118.8578361, 32.0266516, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test25_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(296.94); // 航向角
        y1 = degreesToRadians(21.60);  // 目标偏离角
        x2 = degreesToRadians(2.90);
        y2 = degreesToRadians(45.28);

        VehicleWgs84 car1(118.8579041, 32.0266763, x1, y1);
        VehicleWgs84 car2(118.8578657, 32.0266578, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test34_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(285.00); // 航向角
        y1 = degreesToRadians(18.37);  // 目标偏离角
        x2 = degreesToRadians(16.97);
        y2 = degreesToRadians(16.62);

        VehicleWgs84 car1(118.8579557, 32.0266843, x1, y1);
        VehicleWgs84 car2(118.8578361, 32.0266516, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test35_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(285.00); // 航向角
        y1 = degreesToRadians(18.37);  // 目标偏离角
        x2 = degreesToRadians(2.90);
        y2 = degreesToRadians(45.28);

        VehicleWgs84 car1(118.8579557, 32.0266843, x1, y1);
        VehicleWgs84 car2(118.8578657, 32.0266578, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }

    void test45_car()
    {
        double x1, y1, x2, y2;
        x1 = degreesToRadians(16.97); // 航向角
        y1 = degreesToRadians(16.62); // 目标偏离角
        x2 = degreesToRadians(2.90);
        y2 = degreesToRadians(45.28);

        VehicleWgs84 car1(118.8578361, 32.0266516, x1, y1);
        VehicleWgs84 car2(118.8578657, 32.0266578, x2, y2);
        Vector3d obj_enu;
        obj_enu = get_obj_enu_from_car(car1, car2);

        // 测试转经纬度
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(car1, obj_enu);
    }
} */


void test01_drone()
{
    Vector3d drone_wgs84(118.8563362,32.0266519, 71);
    Vector3d camera_pos(0.10, 0, 0);
    Vector3d pixel(130, 372.5, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(0.7, 0, 0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);

    // 测试转经纬度
    VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);


    Vector3d v1(118.8563330,32.0271650, 0);
    compare_wc(v1, obj_wgs84);
}

void test02_drone()
{
    Vector3d drone_wgs84(118.8561508,32.0263885, 40.40);
    Vector3d camera_pos(0.15, 0, 0);
    Vector3d pixel(321.56, 246.28, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(1.02,0.01,0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);

    // 测试转经纬度
    VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);

    Vector3d v1(118.8563605,32.0267181, 0);
    compare_wc(v1, obj_wgs84);
}
void test03_drone()
{
    Vector3d drone_wgs84(118.8561508,32.0263885, 40.50);
    Vector3d camera_pos(0.2, 0, 0);
    Vector3d pixel(321.56, 246.28, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(1.02,0.01,0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);

    // 测试转经纬度
    VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);

    Vector3d v1(118.8563605,32.0267181, 0);
    compare_wc(v1, obj_wgs84);
}
void test04_drone()
{
    Vector3d drone_wgs84(118.8561508,32.0263885, 40.60);
    Vector3d camera_pos(0.3, 0, 0);
    Vector3d pixel(321.56, 246.28, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(1.02,0.01,0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot);

    // 测试转经纬度
    VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
    Vector3d obj_wgs84;
    obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);

    Vector3d v1(118.8563605,32.0267181, 0);
    compare_wc(v1, obj_wgs84);
}

void test_drone(){
    test01_drone();
    // test02_drone();
    // test03_drone();
    // test04_drone();
}
int main()
{
    // test00_degreesToRadians();
    // test01_get_obj_enu_from_car();
    // test02_get_obj_enu_from_drone();
    // test03_obj_from_enu2wgs84_car();
    // test04_obj_from_enu2wgs84_drone();
    // test05_locate_fusion();
    // test11_car();
    test12_car();
    // test13_car();
    // test14_car();
    // test15_car();
    // test23_car();
    // test24_car();
    // test25_car();
    // test34_car();
    // test35_car();
    // test45_car();
    // test_drone();
}